#include "control/move_control/trajectory_process/trajectory.h"

using namespace control;

void Trajectory::Init(ros::NodeHandle n)
{
    min_diff_ = std::vector<float>{0.05, 10 / 57.3, 10 / 57.3};
}

amr_trajectory_msgs::amr_trajectory_msg Trajectory::SearchOnlinePoint(amr_trajectory_msgs::amr_trajectory_msg &trajectory, AMRState &amr_state)
{
    float min_dis = 1000;
    int min_index = -1;
    int index = 0;
    amr_trajectory_msgs::amr_trajectory_msg path_pick_up;
    amr_trajectory_msgs::point_info point_pick_up;
    if (trajectory.points.empty())
    {
        return path_pick_up;
    }
    for (auto p : trajectory.points)
    {
        float dis = std::hypotf(amr_state.amr_local_.pose.position.x - p.pose.position.x, amr_state.amr_local_.pose.position.y - p.pose.position.y);
        if (dis < min_dis)
        {
            min_dis = dis;
            min_index = index;
        }
        index++;
    }
    if (min_index != -1)
    {
        point_pick_up = trajectory.points[min_index];
        path_pick_up.points.push_back(point_pick_up);
    }
    return path_pick_up;
}
int Trajectory::SearchNearestPoint(amr_trajectory_msgs::amr_trajectory_msg &trajectory, AMRState &amr_state)
{
    if (trajectory.points.empty())
    {
        return -1;
    }

    // 使用当前path_id缩小扫描范围
    std::string target_path_id = amr_state.control_state.cur_path_id;

    // 搜索最小的last_min_index_(當last_min_index是一個無效值)
    if (last_min_index_ < 0)
    {
        for (int i = 0; i < trajectory.points.size(); i++)
        {
            if (trajectory.points[i].pathid == target_path_id)
            {
                last_min_index_ = i;
                break;
            }
            else
            {
                last_min_index_ = 0;
            }
        }
    }
    // last_min_index是有效值，那麼使用last_min_index表示的path_id;min_index搜索範圍就是這個path_id
    else
    {
        target_path_id = trajectory.points[last_min_index_].pathid;
    }
    int min_index = -1;
    float min_dis = INFINITY;

    // 從last_min_index_這個地方開始搜索最近點，通過last_min_index去除已經走過的點
    for (int i = last_min_index_; i < trajectory.points.size(); i++)
    {
        auto &p = trajectory.points[i];

        // 路徑path_id發生變換，或者路徑的運動方向發生變化的時候，停止搜索
        if (p.pathid != target_path_id || p.move_direction != trajectory.points[last_min_index_].move_direction)
        {
            break;
        }

        float dx = amr_state.amr_local_.pose.position.x - p.pose.position.x;
        float dy = amr_state.amr_local_.pose.position.y - p.pose.position.y;
        float dis = sqrt(pow(dx, 2) + pow(dy, 2));
        if (dis < min_dis) // min_index第一個值就是last_min_index_
        {
            min_index = i;
            min_dis = dis;
        }
    }
    if (min_index == -1)
    {
        min_index = last_min_index_;
    }

    // car is stand still，判斷車是否停止
    bool car_stand_still = fabs(amr_state.actuator.walking_speed) < g_zero_speed && fabs(amr_state.actuator.angle_speed) < g_zero_speed;

    static int stand_still_cnt = 0;
    if (car_stand_still)
    {
        stand_still_cnt++;
        if (stand_still_cnt > 5)
        {
            car_stand_still = true;
        }
        else
        {
            car_stand_still = false;
        }
    }
    else
    {
        stand_still_cnt = 0;
    }

    // 静止条件，判斷是否切換min_index的路段（cur_path_id --> nex_path_id)
    for (int i = min_index + 1; i < trajectory.points.size(); i++)
    {
        float d_angle = fabs(common_cal_.AngleConstraintToPI((trajectory.points[i].amr_angle - trajectory.points[min_index].amr_angle)));
        float d_theta = fabs(common_cal_.AngleConstraintToPI(tf::getYaw(trajectory.points[i].pose.orientation) - tf::getYaw(trajectory.points[min_index].pose.orientation)));
        float d_thresh = fabs(common_cal_.Degree2Radian(g_f2r_pose_thea_path_angle_diff_thresh));

        int road_left_point_num = 3;

        // 兩個路段的交接處，要求切換點在前方3個點之中
        if ((i - min_index) > road_left_point_num)
        {
            break;
        }

        if (car_stand_still)
        {
            // jump min index to the start of next road
            // 滿足車子靜止狀態  +  前面3個點以內：角度偏差大  或者 路徑角度偏差大，那麼進行切換 或者 path_id不一樣
            if (car_stand_still && (d_angle > d_thresh || d_theta > d_thresh || trajectory.points[min_index].pathid != trajectory.points[i].pathid))
            {
                min_index = i;
                break;
            }
        }
        else
        {
            // 移动情况下，要求角度和线段角度符合阈值
            if (d_angle <= d_thresh && d_theta <= d_thresh && trajectory.points[min_index].pathid != trajectory.points[i].pathid)
            {
                min_index = i;
                break;
            }
        }
    }
    return min_index; // 运动条件，判断是否切换min_index的路段(cur_path_id --> next_path_id)
}
amr_trajectory_msgs::amr_trajectory_msg Trajectory::PickUpPath(amr_trajectory_msgs::amr_trajectory_msg &trajectory, AMRState &amr_state, int min_index, float pick_up_length)
{
    amr_trajectory_msgs::amr_trajectory_msg path_pick_up;
    amr_trajectory_msgs::point_info point_pick_up;
    float start_distance = 0;
    if (min_index == -1 || trajectory.points.empty())
    {
        return path_pick_up;
    }

    // 假如min_index > path.size
    if (min_index >= trajectory.points.size())
    {
        min_index = trajectory.points.size() - 1;
    }

    // min_index必定會被包含進來
    path_pick_up.points.push_back(trajectory.points[min_index]);
    start_distance = trajectory.points[min_index].distance;
    path_pick_up.points.begin()->distance = 0;

    for (int i = min_index + 1; i < trajectory.points.size(); i++)
    {
        float d_angle = fabs(common_cal_.AngleConstraintToPI((trajectory.points[i].amr_angle - trajectory.points[i - 1].amr_angle)));
        float d_theta = fabs(common_cal_.AngleConstraintToPI(tf::getYaw(trajectory.points[i].pose.orientation) - tf::getYaw(trajectory.points[i - 1].pose.orientation)));
        float d_thresh = fabs(common_cal_.Degree2Radian(g_f2r_pose_thea_path_angle_diff_thresh));
        point_pick_up = trajectory.points[i];
        point_pick_up.distance = point_pick_up.distance - start_distance;
        point_pick_up.target_speed = trajectory.points[i].target_speed;

        // 差異過大的點，不包含進來
        if ((d_angle > d_thresh || d_theta > d_thresh || point_pick_up.distance > pick_up_length))
        {
            // path_pick_up.points.push_back(path.points[i]);
            break;
        }

        path_pick_up.points.push_back(point_pick_up);
    }
    return path_pick_up;
}
amr_trajectory_msgs::point_info Trajectory::SearchAllMapNearestPoint(amr_trajectory_msgs::amr_trajectory_msg &trajectory, AMRState &amr_state)
{
    amr_trajectory_msgs::point_info p_temp;
    p_temp.pathid = "-1";
    int index = 0;
    int min_index = -1;
    float min_dis = 9999;
    float min_record_dis = INFINITY;
    float min_record_theta = INFINITY;
    if (trajectory.points.size() != 0)
    {
        for (auto p : trajectory.points)
        {
            
            float dis = std::hypotf(amr_state.amr_local_.pose.position.x - p.pose.position.x, amr_state.amr_local_.pose.position.y - p.pose.position.y);

            float d_theta_theta = fabs(common_cal_.AngleConstraintToPI(tf::getYaw(amr_state.amr_local_.pose.orientation) - tf::getYaw(p.pose.orientation))); 
            float d_theta_angle = fabs(common_cal_.AngleConstraintToPI(tf::getYaw(amr_state.amr_local_.pose.orientation) - p.amr_angle));

            if (min_record_dis > dis)
            {
                min_record_dis = dis;
                min_record_theta = d_theta_theta;
            }

            bool angle_not_qualified = true;
            for (auto angle_thresh : g_v_search_path_id_angle_thresh)
            {
                if ((d_theta_theta >= angle_thresh.first && d_theta_theta <= angle_thresh.second) || (d_theta_angle >= angle_thresh.first && d_theta_angle <= angle_thresh.second))
                {
                    angle_not_qualified = false;
                    break;
                }
            }

            if (angle_not_qualified || dis > g_search_path_id_dis_thresh || dis > min_dis)
            {
                index++;
                continue;
            }

            min_index = index;
            min_dis = dis;
            index++;
        }

        if (min_index != -1)
        {
            p_temp = trajectory.points[min_index];
            if (min_dis > 0.2)
            {
                p_temp.waypoint_id = "-1";
            }
        }
        else
        {
            ROS_FATAL_STREAM("no find NearestPoint");
            // LOG_WARNING("no find NearestPoint");
        }
    }
    // LOG_INFO("SearchAllMapNearestPoint() --> min_index: {}, min dis: {}, theta: {}", min_index, min_record_dis, min_record_theta);
    return p_temp;
}